#include <ros/ros.h>
#include <tf2/utils.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>

#include <gflags/gflags.h>
#include <glog/logging.h>

#include <mc_path_planning/path_planning_base.h>
#include <mc_path_planning/a_star/a_star.h>

class TestPathGen{
    public:
        TestPathGen();

    private:
        // 地图回调函数
        void MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg);
        // Rviz位姿回调函数
        void RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg);

    private:
        ros::NodeHandle nh_;
        ros::Subscriber rvizPoseSub_;
        ros::Subscriber mapSub_; 
        ros::Publisher mapDilatePub_;
        boost::shared_ptr<PathPlanningBase> globalPathPlannerPtr_; 
        vector<geometry_msgs::PoseStamped> poses_;   
};

TestPathGen::TestPathGen(){
    globalPathPlannerPtr_ = boost::make_shared<Astar>();
    // ros 
    rvizPoseSub_ = nh_.subscribe("/move_base_simple/goal", 10, &TestPathGen::RvizPosesCallback, this);
    mapSub_ = nh_.subscribe("/map", 10, &TestPathGen::MapCallback, this);
    mapDilatePub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/map_dilate", 1, true);
}

// 地图回调函数
void TestPathGen::MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg){
    LOG(INFO) << "[MapCallback] Receive a map. width=" << msg->info.width 
        << ", height=" << msg->info.height
        << ", resolution=" << msg->info.resolution;
    globalPathPlannerPtr_->setOccupancyGrid(msg);

    // 简单的地图膨胀 (膨胀半径为0.25)
    globalPathPlannerPtr_->SimpleDilateOccupancyGrid(0.25);

    // 发布膨胀后的地图
    auto map_ptr = boost::make_shared<nav_msgs::OccupancyGrid>();
    if(globalPathPlannerPtr_->getMap(map_ptr, msg->header.frame_id)){
        mapDilatePub_.publish(*map_ptr);
    }
}

// Rviz位姿回调函数
void TestPathGen::RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg){
    LOG(INFO) << "[RvizPosesCallback] Receive a pose. x=" << msg->pose.position.x
        << ", y=" << msg->pose.position.y
        << ", yaw=" << tf2::getYaw(msg->pose.orientation);
    int poses_size = poses_.size();
    switch(poses_.size()){
    case 0:
        poses_.emplace_back(*msg);
        LOG(INFO) << "[RvizPosesCallback] set start point. poses_size: " 
            << poses_.size();
        break;
    case 1:
        poses_.emplace_back(*msg);
        LOG(INFO) << "[RvizPosesCallback] set goal point. poses_size: " 
            << poses_.size();
        // break;
    case 2:
        LOG(INFO) << "[RvizPosesCallback] planning start... poses_size: " 
            << poses_.size();
        {
            // 开始规划路径
            ros::Time startTime = ros::Time::now();
            // Vec2f start = Vec2f(poses_.front().pose.position.x,
            //     poses_.front().pose.position.y);
            // Vec2f goal = Vec2f(poses_.back().pose.position.x,
            //     poses_.back().pose.position.y);
            Pose2D start = msg_convert::ToPose2D(poses_.front().pose);
            Pose2D goal = msg_convert::ToPose2D(poses_.back().pose);
            vector<Pose2D> points;
            globalPathPlannerPtr_->PathPlanning(start, goal, points);
        }
        // 清空数据
        poses_.clear();
        LOG(INFO) << "[RvizPosesCallback] clear. poses_size: " << poses_.size();
        break;
    default:
        // 清空数据
        poses_.clear();
        LOG(INFO) << "[RvizPosesCallback] clear. poses_size: " << poses_.size();
        break;
    }
}



int main(int argc, char** argv){
    FLAGS_alsologtostderr = true;           // 日志同时输出到stderr
    FLAGS_max_log_size = 1;                 // 单个日志文件大小上限（MB）, 如果设置为0将默认为1
	FLAGS_stop_logging_if_full_disk = true; // 当磁盘写满时，停止输出
    FLAGS_minloglevel = google::INFO;    // 只记录大于{}等级的日志
    // FLAGS_log_dir = "/home/xxty/user_config/logs";
    google::InitGoogleLogging(argv[0]);

    ros::init(argc, argv, "path_gen_test");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    ROS_INFO("path_gen_test start...");

    TestPathGen path_gen_tes;
    ros::spin();

    google::ShutdownGoogleLogging();

    return 0;
}